
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __AIM_TASK_H__
#define __AIM_TASK_H__

#ifdef __cplusplus
extern "C" {
#endif

/* Includes ------------------------------------------------------------------*/
#include "struct_typedef.h"
#include "CAN_receive.h"
#include "pid.h"
#include "remote_control.h"

	
/* ******************************************************************
				              	GIMBAL
****************************************************************** */	
//#define GIMBAL_PUSH_KP 20.0F
//#define GIMBAL_PUSH_KI 0.0F
//#define GIMBAL_PUSH_KD 0.01F
//#define CHANNEL_TO_PUSH_SPEED  3
//	
//#define GIMBAL_LIFT_KP 20.0F
//#define GIMBAL_LIFT_KI 0.0F
//#define GIMBAL_LIFT_KD 0.01F
//#define CHANNEL_TO_LIFT_SPEED  3

	
#define GIMBAL_6020_SPEED_KP 240.0F
#define GIMBAL_6020_SPEED_KI 0.0F
#define GIMBAL_6020_SPEED_KD 0.0F

#define GIMBAL_6020_ANGLE_KP 2.0F
#define GIMBAL_6020_ANGLE_KI 0.0F
#define GIMBAL_6020_ANGLE_KD 0.0F

#define GIMBAL_6020_MIN_ANG_SET 685
#define GIMBAL_6020_MAX_ANG_SET 3090
#define CHANNEL_TO_6020_SPEED  3

#define AUTO_2006_KP 0.0F
#define AUTO_2006_KI 0.0F
#define AUTO_2006_KD 0.0F

#define CLAW_3508_KP 0.0F
#define CLAW_3508_KI 0.0F
#define CLAW_3508_KD 0.0F

#define YAW_GYRO_KP 10000.0f
#define YAW_GYRO_KI 0.0f
#define YAW_GYRO_KD 0.0f
#define YAW_GYRO_MAX_OUT 20000.0f
#define YAW_GYRO_MAX_IOUT 1000.0f

#define YAW_ABSOLUTE_ANGLE_KP 10.0f
#define YAW_ABSOLUTE_ANGLE_KI 0.0f
#define YAW_ABSOLUTE_ANGLE_KD 0.0f
#define YAW_ABSOLUTE_ANGLE_MAX_OUT 1000.0f
#define YAW_ABSOLUTE_ANGLE_MAX_IOUT 0.0f

#define PITCH_ABSOLUTE_ANGLE_KP 20.0f
#define PITCH_ABSOLUTE_ANGLE_KI 0.0f
#define PITCH_ABSOLUTE_ANGLE_KD -1.0f
#define PITCH_ABSOLUTE_ANGLE_MAX_OUT 10.0f
#define PITCH_ABSOLUTE_ANGLE_MAX_IOUT 1.0f

#define PITCH_GYRO_KP 10000.0f
#define PITCH_GYRO_KI 0
#define PITCH_GYRO_KD 0
#define PITCH_GYRO_MAX_OUT 20000.0f
#define PITCH_GYRO_MAX_IOUT 1.0f

/* 上弹电机PID,已调 */
#define load_2006_motor_KP 0.02f
#define load_2006_motor_KI 0.2f
#define load_2006_motor_KD 1.0f
#define load_2006_motor_MAX_OUT 8000.0f
#define load_2006_motor_MAX_IOUT 200.0f

#define CLAW_STEER_MID_VALUE 135

//任务初始化 空闲一段时间
#define GIMBAL_TASK_INIT_TIME 201
//yaw,pitch控制通道以及状态开关通道
#define YAW_CHANNEL   2
#define PITCH_CHANNEL 3
#define GIMBAL_MODE_CHANNEL 0
#define AIM_CHANNEL 1
//遥控器通道
#define LEFT_FLAG_CHANNEL 1
#define RIGHT_FLAG_CHANNEL 0
#define LEFT_X_CHANNEL 2	
#define LEFT_Y_CHANNEL 3	
#define RIGHT_X_CHANNEL 0	
#define RIGHT_Y_CHANNEL 1	

//turn 180°
//掉头180 按键
#define TURN_KEYBOARD KEY_PRESSED_OFFSET_F
//turn speed
//掉头云台速度
#define TURN_SPEED    0.04f
//测试按键尚未使用
#define TEST_KEYBOARD KEY_PRESSED_OFFSET_R
//rocker value deadband
//遥控器输入死区，因为遥控器存在差异，摇杆在中间，其值不一定为零
#define RC_DEADBAND   20
#define GIMBAL_RC_DEADBAND 100


#define YAW_RC_SEN    -0.000005f
#define PITCH_RC_SEN  -0.000006f //0.005

#define YAW_MOUSE_SEN   0.00005f
#define PITCH_MOUSE_SEN 0.00015f

#define YAW_ENCODE_SEN    0.01f
#define PITCH_ENCODE_SEN  0.01f

#define AIM_CONTROL_TIME_MS 2

//test mode, 0 close, 1 open
//云台测试模式 宏定义 0 为不使用测试模式
#define GIMBAL_TEST_MODE 0

#define PITCH_TURN  1
#define YAW_TURN    0

//电机码盘值最大以及中值
#define HALF_ECD_RANGE  4096
#define ECD_RANGE       8191
//云台初始化回中值，允许的误差,并且在误差范围内停止一段时间以及最大时间6s后解除初始化状态，
#define GIMBAL_INIT_ANGLE_ERROR     0.1f
#define GIMBAL_INIT_STOP_TIME       100
#define GIMBAL_INIT_TIME            6000
#define GIMBAL_CALI_REDUNDANT_ANGLE 0.1f
//云台初始化回中值的速度以及控制到的角度
#define GIMBAL_INIT_PITCH_SPEED     0.004f
#define GIMBAL_INIT_YAW_SPEED       0.005f

#define INIT_YAW_SET    0.0f
#define INIT_PITCH_SET  0.0f

//云台校准中值的时候，发送原始电流值，以及堵转时间，通过陀螺仪判断堵转
#define GIMBAL_CALI_MOTOR_SET   8000
#define GIMBAL_CALI_STEP_TIME   2000
#define GIMBAL_CALI_GYRO_LIMIT  0.1f

#define GIMBAL_CALI_PITCH_MAX_STEP  1
#define GIMBAL_CALI_PITCH_MIN_STEP  2
#define GIMBAL_CALI_YAW_MAX_STEP    3
#define GIMBAL_CALI_YAW_MIN_STEP    4

#define GIMBAL_CALI_START_STEP  GIMBAL_CALI_PITCH_MAX_STEP
#define GIMBAL_CALI_END_STEP    5

//判断遥控器无输入的时间以及遥控器无输入判断，设置云台yaw回中值以防陀螺仪漂移
#define GIMBAL_MOTIONLESS_RC_DEADLINE 10
#define GIMBAL_MOTIONLESS_TIME_MAX    3000

//电机编码值转化成角度值
#ifndef MOTOR_ECD_TO_RAD
#define MOTOR_ECD_TO_RAD 0.000766990394f //      2*  PI  /8192
#endif

#define load_motor_angle_set_after_init 100000


typedef enum
{
    GIMBAL_MOTOR_RAW = 0, //电机原始值控制
    GIMBAL_MOTOR_GYRO,    //电机陀螺仪角度控制
    GIMBAL_MOTOR_ENCONDE, //电机编码值角度控制
} gimbal_motor_mode_e;

typedef struct
{
    float kp;
    float ki;
    float kd;

    float set;
    float get;
    float err;

    float max_out;
    float max_iout;

    float Pout;
    float Iout;
    float Dout;

    float out;
} gimbal_PID_t;

typedef struct
{
    const motor_measure_t *gimbal_motor_measure;
    gimbal_PID_t gimbal_motor_absolute_angle_pid;
    gimbal_PID_t gimbal_motor_relative_angle_pid;
	pid_type_def gimbal_motor_angle_pid;
	pid_type_def gimbal_motor_speed_pid;
    pid_type_def gimbal_motor_gyro_pid;
    gimbal_motor_mode_e gimbal_motor_mode;
    gimbal_motor_mode_e last_gimbal_motor_mode;
    uint16_t offset_ecd;
    float max_relative_angle; //rad
    float min_relative_angle; //rad

    float relative_angle;     //rad
    float relative_angle_set; //rad
    float absolute_angle;     //rad
    float absolute_angle_set; //rad
	float angle_set;
    float motor_gyro;         //rad/s
    float motor_gyro_set;
    float motor_speed;
    float raw_cmd_current;
    float current_set;
    int16_t given_current;
	
	uint8_t angle_enable;   //是否使能角度环控制

} gimbal_motor_t;

typedef struct
{
    float max_yaw;
    float min_yaw;
    float max_pitch;
    float min_pitch;
    uint16_t max_yaw_ecd;
    uint16_t min_yaw_ecd;
    uint16_t max_pitch_ecd;
    uint16_t min_pitch_ecd;
    uint8_t step;
} gimbal_step_cali_t;

typedef enum
{              //control-current will be sent to CAN bus derectly.
	GIMBAL_RC_CONTROL,  
	GIMBAL_NO_MOVE,
	GIMBAL_PI_CMD,      
	GIMBAL_DEBUG_RAW,   
	
	aiming_load_motor_stay,
	aiming_load_motor_run,
	aiming_load_motor_reset,
	
} gimbal_mode_e; 

typedef struct
{
    const RC_ctrl_t *gimbal_rc_ctrl;
	gimbal_mode_e gimbal_rc_mode;
    const float *gimbal_INT_angle_point;
    const float *gimbal_INT_gyro_point;

	/* 电机 */
	gimbal_motor_t load_2006_motor;			//上弹电机
    gimbal_motor_t gimbal_pitch_motor;		//俯仰角调节电机
	gimbal_motor_t gimbal_yaw_motor;		////航向调节,这个是6020
	
    gimbal_step_cali_t gimbal_cali;
} gimbal_control_t;

//extern gimbal_control_t gimbal_control;


/**
  * @brief          返回yaw 电机数据指针
  * @param[in]      none
  * @retval         yaw电机指针
  */
extern const gimbal_motor_t *get_yaw_motor_point(void);

/**
  * @brief          return pitch motor data point
  * @param[in]      none
  * @retval         pitch motor data point
  */
/**
  * @brief          返回pitch 电机数据指针
  * @param[in]      none
  * @retval         pitch
  */
extern const gimbal_motor_t *get_pitch_motor_point(void);

extern void aim_task(void const *pvParameters);

static void aiming_init( gimbal_control_t *gimbal_control );
static void load_init( gimbal_control_t *gimbal_control );
static void gimbal_PID_init(gimbal_PID_t *pid, float kp, float ki, float kd, float maxout, float max_iout);
static void gimbal_PID_clear(gimbal_PID_t *gimbal_pid_clear);
static void load_motor_set(void);
static void aim_set_mode(void);
static void gimbal_set_control( gimbal_control_t *gimbal_control );
static void aiming_feedback_update(void);
void gimbal_set_mode(gimbal_control_t *gimbal_mode_set);

#ifdef __cplusplus
}
#endif

#endif /* __CAN_H__ */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
